#include "my_lib/steer.h"


steer_t steers[2] = {0};
// SemaphoreHandle_t steer_mutex = NULL;

/// @brief 外设资源初始化
/// @param  
static void SteerTimInit(void)
{
    ATIM_InitTypeDef ATIM_InitStruct;
    ATIM_OCInitTypeDef ATIM_OCInitStruct;
    GPIO_InitTypeDef GPIO_InitStruct;

    __RCC_ATIM_CLK_ENABLE();
    __RCC_GPIOB_CLK_ENABLE();

    GPIO_InitStruct.IT = GPIO_IT_NONE;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pins = GPIO_PIN_13 | GPIO_PIN_14;
    GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
    PB13_AFx_ATIMCH1B();
    PB14_AFx_ATIMCH2B();
    GPIO_Init(CW_GPIOB, &GPIO_InitStruct);


    ATIM_InitStruct.BufferState = DISABLE;
    ATIM_InitStruct.ClockSelect = ATIM_CLOCK_PCLK;
    ATIM_InitStruct.CounterAlignedMode = ATIM_COUNT_MODE_EDGE_ALIGN;
    ATIM_InitStruct.CounterDirection = ATIM_COUNTING_UP;
    ATIM_InitStruct.CounterOPMode = ATIM_OP_MODE_REPETITIVE;
    ATIM_InitStruct.OverFlowMask = DISABLE;
    ATIM_InitStruct.Prescaler = ATIM_Prescaler_DIV64; // 计数时钟1MHz
    ATIM_InitStruct.ReloadValue = 19999;              // 溢出周期20ms
    ATIM_InitStruct.RepetitionCounter = 0;
    ATIM_InitStruct.UnderFlowMask = DISABLE;

    ATIM_Init(&ATIM_InitStruct);

    ATIM_OCInitStruct.BufferState = ENABLE;
    ATIM_OCInitStruct.OCDMAState = DISABLE;
    ATIM_OCInitStruct.OCInterruptSelect = ATIM_OC_IT_UP_COUNTER;
    ATIM_OCInitStruct.OCInterruptState = ENABLE;
    ATIM_OCInitStruct.OCMode = ATIM_OCMODE_PWM1;
    ATIM_OCInitStruct.OCPolarity = ATIM_OCPOLARITY_NONINVERT;
    ATIM_OC2BInit(&ATIM_OCInitStruct);
    ATIM_OC1BInit(&ATIM_OCInitStruct);

    // ATIM_ITConfig(ATIM_CR_IT_OVE, ENABLE);
    ATIM_CH2Config(ATIM_CHxB_CIE, ENABLE);
    ATIM_CH1Config(ATIM_CHxB_CIE, ENABLE);
    ATIM_SetCompare2B(0);
    ATIM_SetCompare1B(0);
    ATIM_Cmd(ENABLE);
    ATIM_CtrlPWMOutputs(ENABLE);
}

/// @brief 判断角度是否在对应舵机允许范围内
/// @param steer_id 
/// @param angle 
/// @return 
uint8_t IsAngleRight(steer_id_enum_t steer_id, float angle)
{
    if(angle<=steers[steer_id].max_angle && angle>=steers[steer_id].min_angle){
        return 1;
    }
    else{
        return 0;
    }
}

/// @brief 将角度值转换为占空比输出(可用于立即控制舵机)
/// @param steer_id 
/// @param angle 
void SteerAngle2PWM(steer_id_enum_t steer_id, double angle)
{
    uint16_t ccr;
    switch (steer_id)
    {
    case HorizontalSteer:
        ccr = angle/HORIZONTAL_STEER_MECHANICAL_ANGLE*(MAX_ANGLE_DUTY_CCR-MIN_ANGLE_DUTY_CCR) + MIN_ANGLE_DUTY_CCR;
        // __HAL_TIM_SET_COMPARE(&HORIZONTAL_STEER_TIM, HORIZONTAL_STEER_CHANNEL, ccr);
        ATIM_SetCompare1B(ccr);
        break;
    case VerticalSteer:
        ccr = angle/VERTICAL_STEER_MECHANICAL_ANGLE*(MAX_ANGLE_DUTY_CCR-MIN_ANGLE_DUTY_CCR) + MIN_ANGLE_DUTY_CCR;
        // __HAL_TIM_SET_COMPARE(&VERTICAL_STEER_TIM, VERTICAL_STEER_CHANNEL, ccr);
        ATIM_SetCompare2B(ccr);
        break;
    
    default:
        break;
    }   
}

/// @brief 设置舵机目标角度(舵机任务步进控制角度变化)
/// @param steer_id 舵机编号
/// @param angle 0-360
void SetSteerAngle(steer_id_enum_t steer_id, double angle)
{
    if(steer_id>VerticalSteer){
        MY_LOGE("steer", "id is invalid");
        return;
    }
    if(!IsAngleRight(steer_id, angle)){   //一定避免超幅度旋转
        MY_LOGE("steer", "steer[%d] angle[%.1f] is invalid(max:%.1f min:%.1f)", steer_id, angle, steers[steer_id].max_angle, steers[steer_id].min_angle);
        return;
    }
    // xSemaphoreTake(steer_mutex, portMAX_DELAY);
    steers[steer_id].tar_angle = angle;
    steers[steer_id].state = 1;
    // xSemaphoreGive(steer_mutex);                 
}

/////////////////////////////cmd test/////////////////////////////////////
static void HorizontalSteerAngle(double param)
{
    MY_LOGI("steer", "steer horizontal->angle[%.2f]", param);
    SetSteerAngle(HorizontalSteer, param);
}
static void VerticalSteerAngle(double param)
{
    MY_LOGI("steer", "steer Vertical->angle[%.2f]", param);
    SetSteerAngle(VerticalSteer, param);
}
/////////////////////////////cmd test/////////////////////////////////////

/// @brief 启动定时器pwm, 设置初始化角度
/// @param  
void SteerInit(void)
{
    // HAL_TIM_PWM_Start(&HORIZONTAL_STEER_TIM, HORIZONTAL_STEER_CHANNEL);
    // HAL_TIM_PWM_Start(&VERTICAL_STEER_TIM, VERTICAL_STEER_CHANNEL);

    steers[HorizontalSteer].max_angle = HORIZONTAL_STEER_MAX_ANGLE;
    steers[HorizontalSteer].min_angle = HORIZONTAL_STEER_MIN_ANGLE;
    steers[HorizontalSteer].pre_angle = HORIZONTAL_STEER_INIT_ANGLE;  //必要, 防止启动时角度超幅度
    steers[HorizontalSteer].step_angle =HORIZONTAL_STEER_STEP_ANGLE;

    steers[VerticalSteer].max_angle = VERTICAL_STEER_MAX_ANGLE;
    steers[VerticalSteer].min_angle = VERTICAL_STEER_MIN_ANGLE;
    steers[VerticalSteer].pre_angle = VERTICAL_STEER_INIT_ANGLE;
    steers[VerticalSteer].step_angle = VERTICAL_STEER_STEP_ANGLE;

    CmdRegister("STRH",
                "set horiz steer angle like",
                HorizontalSteerAngle);
    CmdRegister("STRV",
                "set verti steer angle like",
                VerticalSteerAngle);
    //舵机旋转至默认初始位置, 角度-1是为了避免初始角度赋值相同不被调度
    SetSteerAngle(HorizontalSteer, HORIZONTAL_STEER_INIT_ANGLE-1);
    SetSteerAngle(VerticalSteer, VERTICAL_STEER_INIT_ANGLE-1);
}

void SteerTask(void* param)
{
    // steer_mutex = xSemaphoreCreateMutex();
    SteerTimInit();
    SteerInit();
    while (1)
    {
        while(steers[0].state || steers[1].state)
        {
            for(uint8_t i=0; i<sizeof(steers)/sizeof(steers[0]); i++)
            {
                // xSemaphoreTake(steer_mutex, portMAX_DELAY);
                if(steers[i].pre_angle == steers[i].tar_angle){
                    steers[i].state = 0;
                    continue;
                }
                else if(fabs(steers[i].pre_angle-steers[i].tar_angle) < steers[i].step_angle){
                    steers[i].pre_angle = steers[i].tar_angle;
                }
                else if(steers[i].tar_angle > steers[i].pre_angle){
                    steers[i].pre_angle += steers[i].step_angle;
                }
                else
                {
                    steers[i].pre_angle -= steers[i].step_angle;         
                }
                // xSemaphoreGive(steer_mutex);    
                SteerAngle2PWM(i, steers[i].pre_angle);
            }
            vTaskDelay(ANGLE_ADD_TIME);
        }
        vTaskDelay(10);
    }
}






